/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-19 11:53:14
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-02-20 10:40:59
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\ShanWaiDight\shanwai.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "shanwai.h"
#include <rtthread.h>
#include <rtdevice.h>
// #include <serial.h>
#include "zino.h"
#include "fc_sensors.h"
#include "statecal.h"
#include "usercmd.h"
#include "statecontrol.h"
#include "fc_sensors.h"
#include "motorset.h"
#define DBG_TAG "sw"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include "position.h"

#define SAMPLE_UART_NAME "uart4"
rt_device_t serial4;

union digitalOscilloscope
{
    float _f8;
    uint32_t _u32;
    int32_t _i32;
    int16_t _i16;
    uint16_t _u16;
    int8_t _i8;
    uint8_t _u8;
    char c[4];
} doTmp;
uint8_t getType(char c)
{

    switch (c)
    {
    case 'l':
        return 2;
        break;
    case 'w':
        return 3;
        break;
    case 'b':
        return 4;
        break;

    default:
        break;
    }
    return 0;
}

void digitalOsc_Head()
{
    doTmp.c[0] = 0x03;
    doTmp.c[1] = 0xfc;
    rt_kput(doTmp.c, 2);
}
/**
 * @brief
 *
 * @param fmt 数据格式：fx,ilx,iwx,ibx,ulx,uwx,ubx, f as float, l as long, w as int, b as byte, x=1~8
 * @param ...
 */
void digitalOsc_printf(const char *fmt, ...)
{
    uint8_t type = 0;
    uint8_t argsCnt = 0;
    va_list args;
    va_start(args, fmt);
    switch (*fmt)
    {
    case 'f':
        type = 1;
        argsCnt = *(fmt + 1) - '0';
        break;
    case 'i':
        type = getType(*(fmt + 1));
        type += 10;
        argsCnt = *(fmt + 2) - '0';
        break;
    case 'u':
        type = getType(*(fmt + 1));
        argsCnt = *(fmt + 2) - '0';
        break;
    default:
        break;
    }
    // doTmp.c[0] = 0x03;
    // doTmp.c[1] = 0xfc;
    // rt_kput(doTmp.c, 2);
    // rt_kprintf("T:%d c:%d\n",type,argsCnt);

    for (uint8_t i = 0; i < argsCnt; i++)
    {
        switch (type)
        {
        case 1:
            doTmp._f8 = va_arg(args, double);
            rt_kput(doTmp.c, 4);
            break;
        case 2:
            doTmp._u32 = va_arg(args, long);
            rt_kput(doTmp.c, 4);
            break;
        case 3:
            doTmp._u16 = va_arg(args, int);
            rt_kput(doTmp.c, 2);
            break;
        case 4:
            doTmp._u8 = va_arg(args, int);
            rt_kput(doTmp.c, 1);
            break;
        case 12:
            doTmp._i32 = va_arg(args, long);
            rt_kput(doTmp.c, 4);
            break;
        case 13:
            doTmp._i16 = va_arg(args, int);
            rt_kput(doTmp.c, 2);
            break;
        case 14:
            doTmp._i8 = va_arg(args, int);
            rt_kput(doTmp.c, 1);
            break;

        default:
            break;
        }
    }
    va_end(args);
    // doTmp.c[0] = 0xfc;
    // doTmp.c[1] = 0x03;
    // rt_kput(doTmp.c, 2);
}
void digitalOsc_End()
{
    doTmp.c[0] = 0xfc;
    doTmp.c[1] = 0x03;
    rt_kput(doTmp.c, 2);
}
void digitalOsc_float(float f)
{
    doTmp._f8 = f;
    rt_kput(doTmp.c, 4);
    // rt_kput(doTmp.c[3],1);
    // rt_kput(doTmp.c[2],1);
    // rt_kput(doTmp.c[1],1);
    // rt_kput(doTmp.c[0],1);
}
void bt_digitalOsc_float(float f)
{
    doTmp._f8 = f;
    rt_device_write(serial4, 0, doTmp.c, 4);
}
void digitalOsc_int16(int16_t i)
{
    doTmp._i16 = i;
    rt_kput(doTmp.c, 2);
}
void digitalOsc_int32(int32_t i)
{
    doTmp._i32 = i;
    rt_kput(doTmp.c, 4);
}
void digitalOsc_int8(int8_t i)
{
    doTmp._i8 = i;
    rt_kput(doTmp.c, 1);
}
void digitalOsc_uint16(uint16_t i)
{
    doTmp._u16 = i;
    rt_kput(doTmp.c, 2);
}
void digitalOsc_uint32(uint32_t i)
{
    doTmp._u32 = i;
    rt_kput(doTmp.c, 4);
}
void digitalOsc_uint8(uint8_t i)
{
    doTmp._u8 = i;
    rt_kput(doTmp.c, 1);
}
union ditalOsc_f8
{
    struct
    {
        uint8_t head[2];
        float f[8];
        uint8_t tail[8];
    } __packed data;
    uint8_t c[36];
} dof8;
void bt_digitalOsc_show_f8(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8)
{
    dof8.data.head[0] = 0x03;
    dof8.data.head[1] = 0xfc;
    dof8.data.f[0] = f1;
    dof8.data.f[1] = f2;
    dof8.data.f[2] = f3;
    dof8.data.f[3] = f4;
    dof8.data.f[4] = f5;
    dof8.data.f[5] = f6;
    dof8.data.f[6] = f7;
    dof8.data.f[7] = f8;
    dof8.data.tail[0] = 0xfc;
    dof8.data.tail[1] = 0x03;
    rt_device_write(serial4, 0, dof8.c, 36);
}
void BT_digitalOsc_Head()
{
    doTmp.c[0] = 0x03;
    doTmp.c[1] = 0xfc;
    rt_device_write(serial4, 0, doTmp.c, 2);
}
void bt_digitalOsc_Head()
{
    doTmp.c[0] = 0x03;
    doTmp.c[1] = 0xfc;
    rt_device_write(serial4, 0, doTmp.c, 2);
}
void BT_digitalOsc_End()
{
    doTmp.c[0] = 0xfc;
    doTmp.c[1] = 0x03;
    rt_device_write(serial4, 0, doTmp.c, 2);
}
void bt_digitalOsc_End()
{
    doTmp.c[0] = 0xfc;
    doTmp.c[1] = 0x03;
    rt_device_write(serial4, 0, doTmp.c, 2);
}
void bt_printf(const char *fmt, ...)
{
    va_list args;
    rt_size_t length;
    static char rt_log_buf[RT_CONSOLEBUF_SIZE];
    va_start(args, fmt);
    /* the return value of vsnprintf is the number of bytes that would be
     * written to buffer had if the size of the buffer been sufficiently
     * large excluding the terminating null byte. If the output string
     * would be larger than the rt_log_buf, we have to adjust the output
     * length. */
    length = rt_vsnprintf(rt_log_buf, sizeof(rt_log_buf) - 1, fmt, args);
    if (length > RT_CONSOLEBUF_SIZE - 1)
        length = RT_CONSOLEBUF_SIZE - 1;
    rt_device_write(serial4, 0, rt_log_buf, length);
    va_end(args);
}
void BT_digitalOsc_printf(const char *fmt, ...)
{
    uint8_t argsCnt = 0;
    va_list args;
    va_start(args, fmt);
    argsCnt = *(fmt + 1) - '0';
    for (uint8_t i = 0; i < argsCnt; i++)
    {
        doTmp._f8 = va_arg(args, double);
        rt_device_write(serial4, 0, &doTmp.c, 4);
    }
    va_end(args);
}

static char *toUpper(char *str)
{
    while (*str)
    {
        if (*str >= 'a' && *str <= 'z')
        {
            *str -= 'a';
            *str += 'A';
        }
        str++;
    }
    return str;
}
extern pid_obj_t pidData[PID_ANGLE_PITCH];
extern pid_obj_t pidData[PID_ANGLE_ROLL];
extern pid_obj_t pidData[PID_ANGLE_YAW];

extern pid_obj_t pidData[PID_RATE_ROLL];
extern pid_obj_t pidData[PID_RATE_PITCH];
extern pid_obj_t pidData[PID_RATE_YAW];

extern pid_obj_t pidData[PID_POSITION_VX];
extern pid_obj_t pidData[PID_POSITION_VY];
extern pid_obj_t pidData[PID_POSITION_VZ];

extern pid_obj_t pidData[PID_POSITION_X];
extern pid_obj_t pidData[PID_POSITION_Y];
extern pid_obj_t pidData[PID_POSITION_Z];
char rxBuff[64] = {0};
uint8_t rxdone = 0;
static bool debugOn = false;
rt_err_t rxIndicate(rt_device_t dev, rt_size_t size)
{
    rt_device_read(dev, 0, rxBuff, size);
    rxdone = 1;
    return 0;
}
// #define PTERM_SCALE 0.032029f
// #define ITERM_SCALE 0.244381f
// #define DTERM_SCALE 0.000529f
static rt_err_t uart_rx_handle(void)
{
    const char ITEM[4][9] = {"Angle", "Rate", "Position", "Velocity"};
    const char AXIS[3] = {'X', 'Y', 'Z'};
    const char PARA[3] = {'P', 'I', 'D'};
    const float SCALE[3] = {0.032029f, 0.244381f, 0.000529f};

    static float *pidItem[4][9] =
        {
            {
                &pidData[PID_ANGLE_PITCH].kp,
                &pidData[PID_ANGLE_PITCH].ki,
                &pidData[PID_ANGLE_PITCH].kd,
                &pidData[PID_ANGLE_ROLL].kp,
                &pidData[PID_ANGLE_ROLL].ki,
                &pidData[PID_ANGLE_ROLL].kd,
                &pidData[PID_ANGLE_YAW].kp,
                &pidData[PID_ANGLE_YAW].ki,
                &pidData[PID_ANGLE_YAW].kd,
            },
            {
                &pidData[PID_RATE_PITCH].kp,
                &pidData[PID_RATE_PITCH].ki,
                &pidData[PID_RATE_PITCH].kd,
                &pidData[PID_RATE_ROLL].kp,
                &pidData[PID_RATE_ROLL].ki,
                &pidData[PID_RATE_ROLL].kd,
                &pidData[PID_RATE_YAW].kp,
                &pidData[PID_RATE_YAW].ki,
                &pidData[PID_RATE_YAW].kd,
            },
            {
                &pidData[PID_POSITION_X].kp,
                &pidData[PID_POSITION_X].ki,
                &pidData[PID_POSITION_X].kd,
                &pidData[PID_POSITION_Y].kp,
                &pidData[PID_POSITION_Y].ki,
                &pidData[PID_POSITION_Y].kd,
                &pidData[PID_POSITION_Z].kp,
                &pidData[PID_POSITION_Z].ki,
                &pidData[PID_POSITION_Z].kd,
            },
            {
                &pidData[PID_POSITION_VX].kp,
                &pidData[PID_POSITION_VX].ki,
                &pidData[PID_POSITION_VX].kd,
                &pidData[PID_POSITION_VY].kp,
                &pidData[PID_POSITION_VY].ki,
                &pidData[PID_POSITION_VY].kd,
                &pidData[PID_POSITION_VZ].kp,
                &pidData[PID_POSITION_VZ].ki,
                &pidData[PID_POSITION_VZ].kd,
            },
        };

    if (rt_strncmp(rxBuff, "db1", 3) == 0)
    {
        debugOn = true;
        bt_printf("debugOn");
    }
    if (rt_strncmp(rxBuff, "db0", 3) == 0)
    {
        debugOn = false;
        bt_printf("debugOff");
    }

    toUpper(rxBuff);
    // rt_kprintf("Rxbuff:%s\n",rxBuff);
    if (rt_strncmp(rxBuff, "PID", 3) == 0)
    {
        // rt_kprintf("get pid\n");
        for (int i = 0; i < 4; i++)
        {
            if (rxBuff[4] == ITEM[i][0])
            {
                for (int a = 0; a < 3; a++)
                {
                    if (rxBuff[5] == AXIS[a])
                    {
                        for (int p = 0; p < 3; p++)
                        {
                            if (rxBuff[6] == PARA[p])
                            {
                                int16_t param = atoi(&rxBuff[8]);
                                *(pidItem[a][a * 3 + p]) = (float)param * SCALE[p];
                                float v = *(pidItem[a][a * 3 + p]);
                                int n = rt_snprintf(rxBuff, 64, "PID %s %c %c == %d(%d.%03d)\n", ITEM[i], AXIS[a], PARA[p], param, (int)v, (int)(v * 1000) % 1000);
                                rt_device_write(serial4, 0, rxBuff, n);
                                rt_kprintf("PID %s %c %c == %d(%d.%03d)\n", ITEM[i], AXIS[a], PARA[p], (int)param, (int)v, (int)(v * 1000) % 1000);
                                rt_memset(rxBuff, 0, 64);
                                return 0;
                            }
                        }
                    }
                }
            }
        }
    }
    // rtserialtx
    bt_printf("Help: PID [item][axis][pare] [num]\n");
    bt_printf("item -> A:angle, R:rate P:postion V:velocity\n");
    bt_printf("axis: X, Y, Z\n");
    bt_printf("para: P, I, D\n");

    return 0;
}
static void uart_sample()
{
    // rt_err_t ret = RT_EOK;
    /* 查找串口设备 */
    zino_pwr_ctrl(PWR_CTRL_EXT_5V_ON);
    serial4 = rt_device_find("uart4");
    if (!serial4)
    {
        rt_kprintf("find uart4 failed!\n");
        return;
    }
    struct serial_configure config = {
        .baud_rate = BAUD_RATE_115200,
        .bit_order = BIT_ORDER_LSB,
        .data_bits = DATA_BITS_8,
        .flowcontrol = RT_SERIAL_FLOWCONTROL_NONE,
        .parity = PARITY_NONE,
        .stop_bits = STOP_BITS_1,
        .bufsz = 64,
    };
    rt_device_control(serial4, RT_DEVICE_CTRL_CONFIG, &config);
    // rt_device_open(serial4, RT_DEVICE_FLAG_RDWR);
    rt_device_open(serial4, RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_DMA_RX | RT_DEVICE_FLAG_DMA_TX);
    rt_device_set_rx_indicate(serial4, rxIndicate);
    /* 发送字符串 */
    // rt_device_write(serial4, 0, str, (sizeof(str) - 1));
    while (1)
    {

        // rt_device_write(serial4, 0, "hello world\n", 13);
        // rt_device_read(serial4,0,)

        // if (debugOn)
        // {
        //     BT_digitalOsc_Head();

        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_ROLL].outP);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_ROLL].outI);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_ROLL].outD);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_ROLL].out);

        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_PITCH].outP);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_PITCH].outI);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_PITCH].outD);
        //     BT_digitalOsc_printf("f1", pidData[PID_RATE_PITCH].out);

        //     BT_digitalOsc_End();
        // }

        // if (rxdone)
        // {
        //     rxdone = 0;
        //     uart_rx_handle();
        // }
        // extern float getThrustLpf();
        bt_digitalOsc_show_f8(
            // pidData[PID_RATE_PITCH].kp/PTERM_SCALE,
            // pidData[PID_RATE_ROLL].kp/PTERM_SCALE,
            // fc_sensor_data.optf_raw.optQual,
            // fc_sensor_data.optf_raw.shutter,
            // get_vbat_voltage_mv(),
            // pidData[PID_POSITION_VZ].out,
            // control.thrust,
            // getThrustLpf()
            // setPoint.Angle.pitch,
            // attitudeExpect.roll,
            // zdrone_state.attitude.pitch,
            // zdrone_state.attitude.roll,
            // setPoint.AngleRate.pitch,
            // attitudeRateExpect.roll,
            // zdrone_state.attitudeRate.roll,
            // control.roll,
            // zdrone_state.attitudeRate.pitch,
            // setPoint.Vel.z,
            // zdrone_state.vel.z,
            // setPoint.Pos.z,
            // zdrone_state.pos.z,
            // usercmd.rcChannel[RC_LUD],
            // usercmd.rcChannel[RC_LLR],
            // usercmd.rcChannel[RC_RLR],
            // usercmd.rcChannel[RC_RUD],
            // fc_sensor_data.tof.quality,
            // fc_sensor_data.tof.range / 10.f,
            // ReachTakingOffHeight,
            // ReachTakingOffHeight,
            // control.thrust,
            // motoroutput[0],
            // motoroutput[1],
            // motoroutput[2],
            // motoroutput[3],
            // 0,
            // 0,
            // setPoint.Angle.yaw,
            // zdrone_state.attitude.yaw,
            // setPoint.AngleRate.yaw,
            // zdrone_state.attitudeRate.yaw,
            // control.yaw
            setPoint.Vel.x,
            zdrone_state.vel.x,
            setPoint.Pos.x,
            zdrone_state.pos.x,
            // setPoint.Angle.yaw,
            // zdrone_state.attitude.yaw,
            // usercmd.rcChannel[RC_LLR],
            // setPoint.self.Pos.x
            setPoint.Vel.y,
            zdrone_state.vel.y,
            // fc_sensor_data.optf_raw.lpfX,
            // fc_sensor_data.optf_raw.optVelX,
            // fc_sensor_data.optf_raw.lpfY,
            // setPoint.self.Pos.x,
            zdrone_state.pos.y,
            setPoint.Pos.y
            // fc_sensor_data.optf_raw.optQual,
            // fc_sensor_data.optf_raw.shutter,
            // fc_sensor_data.optf_raw.is_valid,
            // setPoint.self.Vel.y,
            // zdrone_state.vel.y,
            // setPoint.self.Pos.y,
            // zdrone_state.pos.y
            // 0,
            // zroneEstimator.acc[X],
            // zroneEstimator.acc[Y],
            // zroneEstimator.acc[Z],
            // fc_sensor_data.optf_raw.fixVelX,
            // fc_sensor_data.optf_raw.fixVelY,
            // fc_sensor_data.optf_raw.fixPosX,
            // fc_sensor_data.optf_raw.fixPosY,
            // zdrone_state.vel.x,
            // setPoint.Vel.x,
            // zdrone_state.pos.x,
            // setPoint.Pos.x,
            // zdrone_state.pos.x,
            // setPoint.Vel.y,
            // zdrone_state.vel.y,
            // setPoint.Pos.y,
            // zdrone_state.pos.y

            // setPoint.Vel.z,
            // zdrone_state.vel.z,
            // setPoint.Pos.z,
            // fc_sensor_data.tof.range / 10.0f,
            // fc_sensor_data.tof.quality,
            // zdrone_state.pos.z
            // fc_sensor_data.tof.range/10.0f
            // fc_sensor_data.acc.accdata.x,
            // fc_sensor_data.acc.accdata.y,
            // fc_sensor_data.acc.accdata.z,
            // fc_sensor_data.gyro.gyrodata.x,
            // fc_sensor_data.gyro.gyrodata.y,
            // fc_sensor_data.gyro.gyrodata.z,
            // fc_sensor_data.baro.altitude,
            // 0,
            // 0,
            // 0,
            // 0

        );
        // bt_digitalOsc_show_f8(
        //     zdrone_state.attitudeRate.pitch,
        //     zdrone_state.attitudeRate.roll,
        //     zdrone_state.attitude.pitch,
        //     zdrone_state.attitude.roll,
        //     zdrone_state.acc.x,
        //     zdrone_state.acc.y,
        //     0,
        //     0
        //     // pidData[PID_POSITION_VX].outP,
        //     // pidData[PID_POSITION_VX].inter,
        //     // pidData[PID_POSITION_VX].out,
        //     // pidData[PID_POSITION_VY].outP,
        //     // pidData[PID_POSITION_VY].inter,
        //     // pidData[PID_POSITION_VY].out
        // );

        // digitalOsc_Head();

        // digitalOsc_int16(fc_sensor_data.optf_raw.optVelX);
        // digitalOsc_int16(fc_sensor_data.optf_raw.optVelY);

        // digitalOsc_End();

        rt_thread_mdelay(50);
        // BT_digitalOsc_End();
    }
}

int uart4_task_entry()
{
    rt_thread_t uart4_thread = rt_thread_create("uart4_thread", uart_sample, RT_NULL, 1024, 10, 10);
    if (uart4_thread != RT_NULL)
    {
        rt_thread_startup(uart4_thread);
    }
    else
    {
        LOG_E("uart4_thread create failed.");
    }
    LOG_I("uart4_task_entry");
    return 0;
}
// MSH_CMD_EXPORT(uart_sample, uart4 sample);
ZINO_APP_EXPORT(uart4_task_entry);
